/**********************************************************************
 Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved.
***********************************************************************/
#include <iostream>
#include <unistd.h>
#include <csignal>
#include <sched.h>

#include "control/ControlFrame.h"
#include "control/CtrlComponents.h"
#include "Gait/WaveGenerator.h"
#include "control/BalanceCtrl.h"

#include "interface/KeyBoard.h"
#include "interface/IOROS.h"


bool running = true;

/// @brief over watch the ctrl+c command
void ShutDown(int sig)
{
    std::cout << "stop the controller" << std::endl;
    running = false;
}

/// @brief 设置程序进程调度策略为FIFO, 调度优先级为最高; 需要sudo/root权限
void setProcessScheduler()
{
    pid_t pid = getpid();
    sched_param param;
    param.sched_priority = sched_get_priority_max(SCHED_FIFO);
    if (sched_setscheduler(pid, SCHED_FIFO, &param) == -1)
    {
        std::cout << "[ERROR] Function setProcessScheduler failed." << std::endl;
    }
}

int main(int argc, char **argv)
{
#ifdef USE_NEWBEE2
    std::cout<<"the short-leg newBee2 model is used"<<std::endl;
# else
    std::cout<<"newBee1 model is used"<<std::endl;
#endif

    /* set real-time process */
    setProcessScheduler();
    /* set the print format */
    std::cout << std::fixed << std::setprecision(3);

    ros::init(argc, argv, "unitree_gazebo_servo");

    IOInterface *ioInter;
    CtrlPlatform ctrlPlat;

    ioInter = new IOROS();              //gazebo_ros接口
    ctrlPlat = CtrlPlatform::GAZEBO;

    /***********************************ctrlcomponents init*************************************/

    CtrlComponents *ctrlComp = new CtrlComponents(ioInter);
    ctrlComp->ctrlPlatform = ctrlPlat;
    ctrlComp->dt = 0.002;                   // run at 500hz
    ctrlComp->running = &running;
#ifdef USE_NEWBEE2
    ctrlComp->robotModel=new newBee2_Robot();    //specify robot model
#else
    ctrlComp->robotModel=new newBee_Robot();    //specify robot model
#endif

#define TROT            0
#define CRAWL           1
#define WALKING_TROT    2
#define RUNNING_TROT    3
#define PRONK           4

#define GAIT            TROT            //select 1 gait in 5
#if (GAIT==TROT)
    ctrlComp->waveGen = new WaveGenerator(0.45, 0.5, Vec4(0, 0.5, 0.5, 0)); // Trot
#elif (GAIT==CRAWL)
    ctrlComp->waveGen = new WaveGenerator(1.1, 0.75, Vec4(0, 0.25, 0.5, 0.75));  //Crawl, only for sim
#elif (GAIT==WALKING_TROT)
    ctrlComp->waveGen = new WaveGenerator(0.4, 0.6, Vec4(0, 0.5, 0.5, 0));  //Walking Trot, only for sim
#elif (GAIT==RUNNING_TROT)
    ctrlComp->waveGen = new WaveGenerator(0.4, 0.35, Vec4(0, 0.5, 0.5, 0));  //Running Trot, only for sim
#elif (GAIT==PRONK)
    ctrlComp->waveGen = new WaveGenerator(0.4, 0.7, Vec4(0, 0, 0, 0));  //Pronk, only for sim
#endif
#undef TROT         
#undef CRAWL        
#undef WALKING_TROT 
#undef RUNNING_TROT 
#undef PRONK        

    ctrlComp->geneObj();        //partical property init

    ControlFrame ctrlFrame(ctrlComp);   //control frame

    signal(SIGINT, ShutDown);

    while (running)
    {
        ctrlFrame.run();
    }

    delete ctrlComp;
    return 0;
}
